"""和MATLAB对比，以测试系统的正确性"""
import numpy as np
import scipy.io as scio
import matplotlib.pyplot as plt
from system_odes import Bridge

M_result = scio.loadmat('./dstates_dt.mat')# 17阶时的仿真结果
M_T = M_result['T']
M_states = M_result['states']
M_dstates_dt = M_result['dstates_dt']

on_off = [1e8, 1]
switching = np.array([
    on_off[0], on_off[0], on_off[1],
    on_off[0], on_off[0], on_off[1]], dtype=np.float64)
C_pi = np.array([2e-12, 4e-12, 8e-12, 2e-12, 4e-12, 8e-12], dtype=np.float64)
C_p = np.array([243.3e-12, 243.3e-12], dtype=np.float64)
bridge_rc = Bridge(switching, C_pi, C_p)

# ---------------检验jacobian()的正确性------------------------
dstates_dt = np.empty((M_states.shape[0], 3),dtype=np.float64)
dstates_dt[:,0] = bridge_rc.bridge_odes(M_T[0,10], M_states[:, 10])
A = bridge_rc.jacobian(M_T[0,10], M_states[:, 10])
dstates_dt[:,1] = A@M_states[:, 10]
# X' = AX + BR(t)
# 两者的差异应该只来自BR(t)，所以需要检查不相等部分是否具有非零输入量的影响
dstates_dt[:,2] = dstates_dt[:,0] - dstates_dt[:,1]
print(dstates_dt)

# # ---------------检验bridge_odes()的正确性-----------------
# dstates_dt = np.empty(M_dstates_dt.shape, dtype=np.float64)
# # n = M_states.shape[1]# MATLAB保存的文件是加了噪声的情况，n大了看不清
# n = 60
# for index in range(0, n):
#     dstates_dt[:,index]=bridge_rc.bridge_odes(M_T[0,index],M_states[:,index])
# # 计算相对误差可能出现除零的问题
# fig,axes = plt.subplots(nrows=3, ncols=2, figsize=(25/2.54,35/2.54))
# axes[0,0].plot(M_T[0,:n], \
#     (dstates_dt[0,:n]-M_dstates_dt[0,:n]), label='dIp1_dt')
# axes[0,0].plot(M_T[0,:n], \
#     (dstates_dt[1,:n]-M_dstates_dt[1,:n]), label='dIp2_dt')
# axes[0,0].plot(M_T[0,:n], \
#     (dstates_dt[2,:n]-M_dstates_dt[2,:n]), label='dIs_dt')
# axes[0,0].grid()
# axes[0,0].legend(fontsize=8)
# axes[0,1].plot(M_T[0,:n], M_dstates_dt[0, :n], label='M_dIp1_dt')
# axes[0,1].plot(M_T[0,:n], M_dstates_dt[1, :n], label='M_dIp2_dt')
# axes[0,1].plot(M_T[0,:n], M_dstates_dt[2, :n], label='M_dIs_dt')
# axes[0,1].grid()
# axes[0,1].legend(fontsize=8)
# axes[1,0].plot(M_T[0,:n], \
#     (dstates_dt[3, :n]-M_dstates_dt[3, :n]), label='dQeq1_dt')
# # 下面这个相对差别较大(索引问题，已修复)
# axes[1,0].plot(M_T[0,:n], \
#     (dstates_dt[4, :n]-M_dstates_dt[4, :n]), label='dQeq2_dt')
# axes[1,0].grid()
# axes[1,0].legend(fontsize=8)
# axes[1,1].plot(M_T[0,:n], M_dstates_dt[3, :n], label='M_dQeq1_dt')
# axes[1,1].plot(M_T[0,:n], M_dstates_dt[4, :n], label='M_dQeq2_dt')
# axes[1,1].grid()
# axes[1,1].legend(fontsize=8)
# axes[2,0].plot(M_T[0,:n], \
#     (dstates_dt[5, :n]-M_dstates_dt[5, :n]), label='dQpri11_dt')
# axes[2,0].plot(M_T[0,:n], \
#     (dstates_dt[6, :n]-M_dstates_dt[6, :n]), label='dQpri12_dt')
# axes[2,0].plot(M_T[0,:n], \
#     (dstates_dt[7, :n]-M_dstates_dt[7, :n]), label='dQpri13_dt')
# axes[2,0].grid()
# axes[2,0].legend(fontsize=8)
# axes[2,1].plot(M_T[0,:n], M_dstates_dt[5, :n], label='M_dQpri11_dt')
# axes[2,1].plot(M_T[0,:n], M_dstates_dt[6, :n], label='M_dQpri12_dt')
# axes[2,1].plot(M_T[0,:n], M_dstates_dt[7, :n], label='M_dQpri13_dt')
# axes[2,1].grid()
# axes[2,1].legend(fontsize=8)
# fig,axes = plt.subplots(2, 2, figsize=(25/2.54,30/2.54))
# axes[0,0].plot(M_T[0,:n], \
#     (dstates_dt[-6,:n]-M_dstates_dt[-6,:n]), label='dUo1_dt')
# axes[0,0].plot(M_T[0,:n], \
#     (dstates_dt[-7,:n]-M_dstates_dt[-7,:n]), label='d2Uo1_dt2')
# axes[0,0].grid()
# axes[0,0].legend(fontsize=8)
# axes[0,1].plot(M_T[0,:n], M_dstates_dt[-6,:n], label='M_dUo1_dt')
# axes[0,1].plot(M_T[0,:n], M_dstates_dt[-7,:n], label='M_d2Uo1_dt2')
# axes[0,1].grid()
# axes[0,1].legend(fontsize=8)
# axes[1,0].plot(M_T[0,:n], \
#     (dstates_dt[-2,:n]-M_dstates_dt[-2,:n]), label='dUlp_dt')
# axes[1,0].plot(M_T[0,:n], \
#     (dstates_dt[-1,:n]-M_dstates_dt[-1,:n]), label='d2Ulp_dt2')
# axes[1,0].grid()
# axes[1,0].legend(fontsize=8)
# axes[1,1].plot(M_T[0,:n], M_dstates_dt[-2,:n], label='M_dUlp_dt')
# axes[1,1].plot(M_T[0,:n], M_dstates_dt[-1,:n], label='M_d2Ulp_dt2')
# axes[1,1].grid()
# axes[1,1].legend(fontsize=8)
# plt.show()
